#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry, Path
from geometry_msgs.msg import PoseStamped
import numpy as np
import acado
import time
import matplotlib.pyplot as plt

from trajectory_msgs.msg import  MultiDOFJointTrajectory,MultiDOFJointTrajectoryPoint
from sensor_msgs.msg import JointState
from nav_msgs.msg import Path
from geometry_msgs.msg import Transform, Twist,PoseWithCovarianceStamped
from ros_numpy import msgify,numpify

N=10 #timestep
NU=4
NOD=3
NX=16
NY=16
NYN=4
NW=16
NWN=12

class PathPublisher:
    def __init__(self):
        # Initialize ROS node
        rospy.init_node('pos_planning_node')
        
        # Create publisher for Path message
        self.path_pub = rospy.Publisher('/slot/path', Path, queue_size=10)
        self.traj_pub=rospy.Publisher('/position_traj',MultiDOFJointTrajectory,queue_size=10)
        # Create subscribers for pose_stamped_1 and pose_stamped_2
        rospy.Subscriber('/slot/target_pose',PoseWithCovarianceStamped,self.pose_stamped_2_callback)
        # Create an empty Path message object
        self.path_msg = Path()
        
        self.target_position=np.array([3.0,0.0,0.0])
        
        

        
    def pose_stamped_2_callback(self, msg):
        target_pose=numpify(msg.pose.pose)
        pose_tmp=target_pose[:3,3]
        self.target_position[0]=pose_tmp[0]
        self.target_position[1]=pose_tmp[1]
        self.target_position[2]=pose_tmp[2]
        
        
        
        ###
        
        x0=np.zeros(NX).reshape(1,NX)
        u=np.zeros([N,NU])

        Q=np.eye(NW)
        Q=np.diag([0,0,0,1,1,1,1,100,10,10,100,10,1,1,1,5e5])
        W=np.transpose(np.tile(Q,N))
        WN=Q[:-4,:-4]
        y=np.zeros([N,NY])
        
        OD=np.ones([N+1,NOD])
        
        #self.target_position=np.array([3.0,0.1,0.0])
        for i in range(N):
            OD[i,0:3]=self.target_position
            OD[i,2]=0.0
        
        
        print(self.target_position)
        
        x0[0,0]=0
        x0[0,1]=0
        x0[0,2]=0
        x0[0,3]=1.0#qw
        x0[0,4]=0.0#qx
        x0[0,5]=0.0#qy
        x0[0,6]=0.0#qz
        x0[0,7]=0.0#vx
        x0[0,8]=0.0#vy
        x0[0,9]=0.0#vz
        x0[0,10]=1.0#pxx
        x0[0,11]=0.0#pxy
        x0[0,12]=0.0#pxz
        x0[0,13]=1.0#pyy
        x0[0,14]=0.0#pyz
        x0[0,15]=1.0#pzz
    
        X=np.tile(x0.reshape(NX,1),N+1).T
        
        
        #X,u=acado.mpc(0,1,x0,X,u,OD,y,W,WN,0)
        x0=X[1,:].reshape(1,NX)
        
        
        xlog=[]
        tracelog=[]

        t0=time.time()
        
            # Publish the pose stamped message
        path_msg = Path()
        path_msg.header.frame_id = 'body'
        path_msg.header.stamp = rospy.Time.now()
     
            #print("iter: "+str(i))
        for iter in range(1):
            X,u=acado.mpc(0,1,x0,X,u,OD,y,W,WN,1)
            
        
        for i in range(10):
            pose = PoseStamped()
            pose.header = path_msg.header
            pose.pose.position.x = X[i,0]
            pose.pose.position.y = X[i,1]
            pose.pose.position.y = X[i,2]
            pose.pose.orientation.w = 1.0
            pose.header.frame_id='body'
            path_msg.poses.append(pose)
        
        self.path_pub.publish(path_msg)


if __name__ == '__main__':
    try:
        path_publisher = PathPublisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass